/* 
 * File:   Robot.h
 * Author: hadrien
 *
 * Created on 6 mars 2012, 23:26
 */

#ifndef ROBOT_H
#define	ROBOT_H
#include <mrpt/base.h>
#include <mrpt/slam.h>
#include "global.h"

struct Action {
    double velocity;
    double turn_rate;

    Action(double v, double theta) : velocity(v), turn_rate(theta) {
    };

    friend bool operator<(const Action &id1, const Action & id2) {
        return (id1.velocity < id2.velocity) || (id1.velocity == id2.velocity && id1.turn_rate < id2.turn_rate);
    }
};

struct RangeSensorModel {
    float minSensorDistance;
    float maxSensorDistance;
    float fieldOfView_deg;
    float stdRange;
    float stdYaw_deg;
};

struct OdometrySensorModel {
    bool noisy;
    double Axy_err_std;
    double Aphi_err_std_deg;
};

class Robot {
public:
    Robot(float TAU = 0, float DELAY = 0);
    ~Robot(){
        obs.clear_unique();
    }
    Robot(const Robot &orig);
    
    void setDelayModelParams(float TAU_delay_sec = 1.8f, float CMD_delay_sec = 0.3f) {
        cTAU = TAU_delay_sec;
        cDELAY = CMD_delay_sec;
    }

    void setOdometryErrors(const OdometrySensorModel &odo) {
        use_odometry_error = odo.noisy;
        m_Axy_err_std = odo.Axy_err_std;
        m_Aphi_err_std = mrpt::math::DEG2RAD(odo.Aphi_err_std_deg);
        opts.gausianModel.a1 = 0;
        opts.gausianModel.a2 = 0;
        opts.gausianModel.a3 = 0;
        opts.gausianModel.a4 = 0;
        opts.gausianModel.minStdXY = m_Axy_err_std;
        opts.gausianModel.minStdPHI = m_Aphi_err_std;
    }

    void getRealPose(mrpt::poses::CPose2D &pose) const {
        pose = m_pose;
    }

    /** Reads the real robot pose. \sa getOdometry  */
    void getRealPose(mrpt::poses::CPose3D &pose) const {
        pose = mrpt::poses::CPose3D(m_pose);
    }

    void setSensorParameters(const RangeSensorModel &sensor);

    void setRealPose(const mrpt::poses::CPose2D &p) {
        this->m_pose = p;
    }

    void resetStatus();
    void move(const Action& action, double time_interval);
    void sense(mrpt::slam::CActionCollectionPtr &actions, mrpt::slam::CSensoryFramePtr &observations, const mrpt::slam::CLandmarksMap &landmarkMap);

private:

    void movementCommand(double lin_vel, double ang_vel);
    /** This method must be called periodically to simulate discrete time intervals.
     */
    void simulateInterval(double At);
    mrpt::poses::CPose2D m_pose; //!< Global, absolute and error-free robot coordinates
    mrpt::poses::CPose2D m_odometry; //!< Used to simulate odometry (with optional error)

    /** Instantaneous velocity of the robot (linear, m/s)
     */
    double v;

    /** Instantaneous velocity of the robot (angular, rad/s)
     */
    double w;

    /** Simulation time variable
     */
    double t;

    /** Whether to corrupt odometry with noise  */
    bool use_odometry_error;

    /** Dynamic limitations of the robot.
     * Approximation to non-infinity motor forces: A first order low-pass filter, using:
     *   Command_Time: Time "t" when the last order was received.
     *   Command_v, Command_w: The user-desired velocities.
     *   Command_v0, Command_w0: Actual robot velocities at the moment of user request.
     */
    double Command_Time,
    Command_v, Command_w,
    Command_v0, Command_w0;

    /** The time-constants for the first order low-pass filter for the velocities changes. */
    float cTAU; // 1.8 sec

    /** The delay constant for the velocities changes.  */
    float cDELAY;

    double m_Axy_err_std;
    double m_Aphi_err_std;

    mrpt::poses::CPose3D sensorPoseOnRobot;
    mrpt::slam::CActionRobotMovement2D::TMotionModelOptions opts;

    mrpt::poses::CPose2D podoSinceLastObs; // Delta poses

    float m_minSensorDistance;
    float m_maxSensorDistance;
    float m_fieldOfView;
    float m_stdRange;
    float m_stdYaw;
    mrpt::slam::CObservationBearingRangePtr obs;

    double odoScale;
};

#endif	/* ROBOT_H */

